/*
Tag ZBY
------------------------------------------
MPU6050/6500
pitch  roll  yaw accel[0]  gyro[0]
------------------------------------------

--------------------------------------------------------
OLED(HARDWARE SPI)
OLED_ShowString(0,7,(uint8_t *)"MPU6050 Demo",8);
--------------------------------------------------------
*/
#include "main.h"
#include "stdio.h"
#include "ti_msp_dl_config.h"

uint8_t oled_buffer[32];
char mode_str[20];
int mode = 1;
int situation = 1;
int sys_run = 0;
int angel_init = 0;
int cycles = 0;

void motor_run(void) {
  DL_TimerA_startCounter(PWM_MOTOR_INST);
  DL_GPIO_clearPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN2_PIN);
  DL_GPIO_clearPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN2_PIN);
}
void motor_stop(void) {
  DL_TimerA_stopCounter(PWM_MOTOR_INST);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN2_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN2_PIN);
}
void motor_speed_set(uint16_t motor_left_speed, uint16_t motor_right_speed) {
  if ((motor_left_speed > 95) || (motor_right_speed > 95)) {
    return;
  } else {
    motor_left_speed = 100 - motor_left_speed;
    motor_right_speed = 100 - motor_right_speed;
    DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, motor_left_speed * 10,
                                     GPIO_PWM_MOTOR_C0_IDX);
    DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, motor_right_speed * 10,
                                     GPIO_PWM_MOTOR_C1_IDX);
  }
}

int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();
  // MPU6050_Init();
  OLED_Init();
  DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
  while (1) {
    if (DL_GPIO_readPins(GPIO_MODE_SELECT_PIN_SWITCH_PORT,
                         GPIO_MODE_SELECT_PIN_SWITCH_PIN) != 0) {
      if (mode < 5) {
        mode = mode + 1;
        delay_ms(200);
      } else {
        mode = 1;
        delay_ms(200);
      }
    }
    if (DL_GPIO_readPins(GPIO_MODE_SELECT_PIN_RUN_PORT,
                         GPIO_MODE_SELECT_PIN_RUN_PIN) == 0) {
      if (sys_run == 0) {
        sys_run = 1;
        delay_ms(200);
      } else {
        sys_run = 0;
        delay_ms(200);
      }
    }
    if (sys_run == 0) {
      motor_stop();
    }
    OLED_ShowNum(0, 2, mode, 1, 16);
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_1_PIN) ==
        0) {
      OLED_ShowNum(0, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(0, 0, 1, 1, 8);
    }
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_2_PIN) ==
        0) {
      OLED_ShowNum(10, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(10, 0, 1, 1, 8);
    }
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_3_PIN) ==
        0) {
      OLED_ShowNum(20, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(20, 0, 1, 1, 8);
    }
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_4_PIN) ==
        0) {
      OLED_ShowNum(30, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(30, 0, 1, 1, 8);
    }
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_5_PIN) ==
        0) {
      OLED_ShowNum(40, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(40, 0, 1, 1, 8);
    }
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_6_PIN) ==
        0) {
      OLED_ShowNum(50, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(50, 0, 1, 1, 8);
    }
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_7_PIN) ==
        0) {
      OLED_ShowNum(60, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(60, 0, 1, 1, 8);
    }
    if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT, GPIO_SEARCH_LINE_PIN_8_PIN) ==
        0) {
      OLED_ShowNum(70, 0, 0, 1, 8);
    } else {
      OLED_ShowNum(70, 0, 1, 1, 8);
    }
    if (sys_run == 1) {
      int stop_cycles = mode * 4 + 1;
      while (1) {
        motor_run();
        motor_speed_set(15, 15);
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) != 0) {
          motor_speed_set(15, 5);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) != 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) != 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) != 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_9_PIN) != 0) {
          motor_speed_set(5, 15);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) != 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) != 0) {
          motor_speed_set(24, 5);
          DL_GPIO_clearPins(GPIO_MOTOR_CONTROL_PORT,
                            GPIO_MOTOR_CONTROL_PIN_AIN1_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_AIN2_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                            GPIO_MOTOR_CONTROL_PIN_BIN1_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_BIN2_PIN);
          delay_ms(600);
          motor_stop();
          delay_ms(500);
          cycles = cycles + 1;
        }
        if (cycles == stop_cycles) {
          motor_stop();
          sys_run = 0;
          break;
        }
      }
    }
  }
}
